# pragma once

# include "cpp_tools.h"

# ifdef TEST
int PWM_MODE_MS = 0;
int PWM_MS_OUTPUT = 0;
int INPUT=0;
void wiringPiSetupGpio(){};
void pwmSetMode(int){};
void pwmSetRange(int){};
void pwmSetClock(int){};
void pinMode(int, int){};
void pwmWrite(int pin, double width)
{
    std::cout << "write pwm " << width << " at pin: " << pin << std::endl;
};
# else
# include <wiringPi.h>
# endif


namespace ppm
{
    void init()
    {
        wiringPiSetupGpio(); // 这个似乎多线程不安全，所以只在最开始执行一次
        pwmSetMode(PWM_MODE_MS);
        pwmSetRange(1000);
        pwmSetClock(192);
    }

    class controller
    {
    private:
        int pin = 12;
        double pwm_add = 100./3.; // 根据实际转向角度调整
        double pwm_0 = 150; // 根据实际转向角度调整
        double pwm_minus = 100./3.; // 根据实际转向角度调整

        std::thread p_output;

        cpp_tools::mtDataManager<double> mt_u = 0;

        bool isRunning = true;
    
    public:
        /// @brief 
        /// @param arg1 pin
        /// @param arg2 pwm_add
        /// @param arg3 pwm_0
        /// @param arg4 pwm_minus
        controller(int arg1, double arg2, double arg3, double arg4)
            : pin(arg1), pwm_add(arg2), pwm_0(arg3), pwm_minus(arg4)
        {
            p_output = std::move(std::thread([&]() -> void {
                pinMode(pin,PWM_MS_OUTPUT);

                while (isRunning)
                {
                    auto t = std::chrono::steady_clock::now();

                    double u = *mt_u.read().access();

                    double pulseWidth;
                    u = cpp_tools::saturation(u,-1,1);
                    if (u >= 0)
                    {
                        pulseWidth = u*pwm_add + pwm_0;
                    }
                    else
                    {
                        pulseWidth = u*pwm_minus + pwm_0;
                    }

                    pwmWrite(pin,pulseWidth);

                    std::this_thread::sleep_until(t + std::chrono::milliseconds(10));
                }
            }));
        };
    
        ~controller()
        {
            isRunning = false;

            p_output.join();

            pinMode(pin, INPUT);
        };

        void setControl(double control)
        {
            *mt_u.write().access() = control;
        }

        controller(const controller&) = delete;
        controller operator = (const controller&) = delete;
    };
} // namespace ppm


